use std::collections::BTreeMap;

use serde::Deserialize;

use crate::{Error, Result};

#[derive(Clone, Debug, Deserialize)]
pub struct SensorsConfig {
    pub gnss: GNSSConfig,
    pub chassis: ChassisConfig,
    pub obstacles_detector: ObstaclesDetectorConfig,
    pub cameras: BTreeMap<String, CameraConfig>,
}

#[derive(Clone, Debug, Deserialize)]
pub struct GNSSConfig {
    pub period: f64,
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
}

#[derive(Clone, Debug, Deserialize)]
pub struct ChassisConfig {
    pub period: f64,
}

#[derive(Clone, Debug, Deserialize)]
pub struct ObstaclesDetectorConfig {
    pub period: f64,
}

#[derive(Clone, Debug, Deserialize)]
pub struct CameraConfig {
    pub period: f64,
    pub position_xyz: (f64, f64, f64),
    pub orientation_wxyz: (f64, f64, f64, f64),
    pub fov: f64,
    pub image_width: u32,
    pub image_height: u32,
    pub is_gray: bool,
    pub iso: f64,
    pub gamma: f64,
    pub shutter_speed: f64,
}

impl SensorsConfig {
    pub fn parser(config: &[u8]) -> Result<Self> {
        serde_json::from_slice::<Self>(config)
            .map_err(|_| Error::new("Parser sensors config error".into()))
    }

    pub fn default() -> Self {
        Self {
            gnss: GNSSConfig {
                period: 0.01,
                position_xyz: (0.0, 0.0, 0.0),
                orientation_wxyz: (1.0, 0.0, 0.0, 0.0),
            },
            chassis: ChassisConfig { period: 0.01 },
            obstacles_detector: ObstaclesDetectorConfig { period: 0.1 },
            cameras: BTreeMap::new(),
        }
    }

    pub fn default_with_camera() -> Self {
        let mut cameras = BTreeMap::new();
        cameras.insert(
            "camera_0".to_string(),
            CameraConfig {
                period: 1.0 / 60.0,
                position_xyz: (0.0, 0.0, 3.0),
                orientation_wxyz: (0.0, 0.0, 0.0, 0.0),
                fov: 90.0,
                image_width: 800,
                image_height: 600,
                is_gray: false,
                iso: 100.0,
                gamma: 2.2,
                shutter_speed: 200.0,
            },
        );

        Self {
            gnss: GNSSConfig {
                period: 0.01,
                position_xyz: (0.0, 0.0, 0.0),
                orientation_wxyz: (1.0, 0.0, 0.0, 0.0),
            },
            chassis: ChassisConfig { period: 0.01 },
            obstacles_detector: ObstaclesDetectorConfig { period: 0.1 },
            cameras,
        }
    }
}
